function [betaMFF, dqMFOpt, dlambdaMax] = multvecattdeterr(vM, vF, dvM, dvF, qMFOpt, lambdaMax, K, a)
%MULTVECATTDETERR 多矢量定姿误差
%
% Input Arguments:
% # vM: 3*n矩阵，M系下的一组参考矢量，n表示矢量数，单位无
% # vF: 3*n矩阵，F系下的一组参考矢量，n表示矢量数，单位无
% # dvM: 3*n矩阵，vM的误差，单位无
% # dvF: 3*n矩阵，vF的误差，单位无
% # qMFOpt: 4*1向量，qMF真值，单位无
% # lambdaMax: 标量，与qMFOpt对应的K特征值，单位无
% # K: 4*4矩阵，Davenport-q方法中的二次型矩阵，单位无，默认为空（函数内部计算）
% # a: 1*n向量，权重值，默认均为1/n，单位无
%
% Output Arguments:
% # dqMFOpt: 4*1向量，qMF的误差，单位无
% # betaMFF: 3*1向量，与dqMFOpt对应的姿态误差（等效旋转矢量），单位无
% # dlambdaMax: 标量，lambdaMax的误差
%
% References:
% # 《应用导航算法工程基础》“多矢量定姿算法及其误差”

n = size(vM, 2);
if nargin < 8
    a = 1/n * ones(1, n);
end
if nargin < 7
    K = [];
end

assert(size(vF, 2)==n && size(dvM, 2)==n && size(dvF, 2)==n && size(a, 2)==n);

dB = zeros(3);
for k=1:n
    dB = dB + a(1, k) * (dvM(:, k)*vF(:, k)' + vM(:, k)*dvF(:, k)');
end
dz = outer2cross(dB)';
dtrB = trace(dB);
dK = [dtrB, dz'; dz, dB+dB'-dtrB*eye(3)];
if isempty(K)
    K = davqquadmat(vM, vF, a);
end
if nargout > 2
    dlambdaMax = qMFOpt' * dK * qMFOpt;
end
A = lambdaMax*eye(4)-K;
dqMFOpt = lsqminnorm(A, dK*qMFOpt, eps(sum(abs(A(:))))*16);
betaMFF = quatdiff_mult((dqMFOpt+qMFOpt)', qMFOpt', false)';
end